
// F. Chaxel 2010

#include "Motors.h"

#include <iostream>
#include <sys/time.h>
#include <cmath>

using namespace std;

Motors* Motors::_instance = NULL;


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Motors::Motors()
{
	if (_instance!=NULL)	// I am a Singleton
		throw ;
	
	_instance=this;

	Stop();
	InitPosition();
	
	this->sl = 0;
	this->sr = 0;
	
	this->pl = 0;
	this->pr = 0;
	
	//this->Run();
	this->StartRunThreaded();
}

Motors::~Motors()
{
	Stop();
	_instance=NULL;
}

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
int Motors::GetBattery(){
	return 0;
}

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void Motors::InitPosition(){
	//i2c->WriteByte (MotorControlMD23,16,32);	// reset encoder to 0
	this->pl = 0;
	this->pr = 0;
}
//////////////////////////////////////////////////////////////////////
void Motors::Stop()
{
	SetSpeed (128,128);
	cout << "stopped" << endl;
}
//////////////////////////////////////////////////////////////////////
void Motors::SetSpeed (unsigned int Left, unsigned int Right)
{
	this->sl = Left  - 128;
	this->sr = Right - 128;
	std::cout << Left << "/" << Right << std::endl;
}
/////////////////////////////////////////////////////////////////////
void Motors::GetPos(long *Left, long *Right){
	*Left  = this->pl;
	*Right = this->pr;
}

void Motors::Run(){
	timeval t1, t2;
	
	gettimeofday(&t1, NULL);
	
	while(true){
		gettimeofday(&t2, NULL);
		float diff = t2.tv_usec - t1.tv_usec;
		
		if(this->sl != 0){
			int s = this->sl;
			if(s < 0)
				s *= -1;	
			if(diff >= 128/360/s){
				long nbStep = ceil(diff/(128/360/s));
				this->pl += nbStep;
			}
		}		
		if(this->sr != 0){
			int s = this->sl;
			if(s < 0)
				s *= -1;	
			if(diff >= 128/360/s){
				long nbStep = ceil(diff/(128/360/s));
				this->pr += nbStep;
			}
		}
		//.02 m/s
		t1 = t2;
	}
}
